use crate::geom::{AAVec3f, Axis, Direction, Vec3f};

/// An axis-aligned bounding box.  Has a position and dimensions.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct AABB {
    /// Origin point for the AABB.
    base: Vec3f,

    /// Offset from the min corner to the max corner.  Should always be positive
    /// in each component.
    dims: Vec3f,
}

impl AABB {
    fn new_unchecked(base: Vec3f, dims: Vec3f) -> Self {
        Self { base, dims }
    }

    /// Creates a new AABB, rectifying the dims component.
    pub fn new(base: Vec3f, dims: Vec3f) -> Self {
        Self::new_unchecked(base, dims.abs())
    }

    /// Creates a new AABB, centered at the origin, rectifying the dims component.
    pub fn new_origin(dims: Vec3f) -> Self {
        let dims = dims.abs();
        Self::new_unchecked(Vec3f::zero() - dims * 0.5, dims)
    }

    /// Creates a new AABB from a centerpoint, rectifying the dims component.
    pub fn from_center(center: Vec3f, dims: Vec3f) -> Self {
        let dims = dims.abs();
        let base = Vec3f::new(
            center.x() - dims.x() / 2.0,
            center.y() - dims.y() / 2.0,
            center.z() - dims.z() / 2.0,
        );

        Self::new_unchecked(base, dims)
    }

    /// Unit AABB.
    pub fn unit() -> Self {
        Self::new(Vec3f::zero(), Vec3f::new(1.0, 1.0, 1.0))
    }

    pub fn base(&self) -> Vec3f {
        self.base
    }

    pub fn dims(&self) -> Vec3f {
        self.dims
    }

    /// Returns the height of the AABB.
    pub fn height(&self) -> f32 {
        self.dims().y()
    }

    /// Returns the "far corner" of the AABB away from the base.
    pub fn get_far_corner(&self) -> Vec3f {
        self.base + self.dims
    }

    /// Returns the center point of the AABB.
    pub fn get_center(&self) -> Vec3f {
        self.base + (self.dims * 0.5)
    }

    /// Computes the total volume of the AABB, `dx*dy*dz`.
    pub fn get_volume(&self) -> f32 {
        self.dims.x() * self.dims.y() * self.dims.z()
    }

    /// Computes a point lerping within the AABB from 0..1 in each dimension,
    /// but consistent out of bounds.  I don't know why you'd want to do this.
    pub fn lerp_3d(&self, p: Vec3f) -> Vec3f {
        Vec3f::new(
            self.base.x() + p.x() * self.dims.x(),
            self.base.y() + p.y() * self.dims.y(),
            self.base.z() + p.z() * self.dims.z(),
        )
    }

    /// Shifts the AABB by some offset.
    pub fn offset(&self, d: Vec3f) -> Self {
        Self::new(self.base + d, self.dims)
    }

    /// Returns the AABB but shifted to be centered on the origin.
    pub fn to_origin(&self) -> Self {
        // Directly construct to avoid rectifying it again.
        let new_aabb = Self::new_unchecked(self.base() - self.get_center(), self.dims());

        // sanity check
        #[cfg(test)]
        assert!(
            new_aabb.get_center().magnitude() < 0.0001,
            "aabb: not at origin"
        );

        new_aabb
    }

    /// Returns the bounds along one axis.
    pub fn axis_bounds(&self, axis: Axis) -> (f32, f32) {
        let base = self.base.component(axis);
        let dim = self.dims.component(axis);
        (base, base + dim)
    }

    /// Constructs a new instance, adding extents, as needed for Minkowski
    /// collision checking.  Rectifies the dims if it gets inverted, although
    /// that might get weird.
    pub fn expand_extents_by(&self, ext: Vec3f) -> Self {
        Self {
            // Subtract half the new extent to expand it out...
            base: self.base() - (ext * 0.5),

            // ...then add in the additional diff.
            dims: (self.dims + ext).abs(),
        }
    }

    /// Checks if the AABB contains the point, as if it's an open interval (uses
    /// noninclusive comparisons).
    pub fn contains(&self, pos: Vec3f) -> bool {
        let far = self.get_far_corner();
        pos.x() > self.base.x()
            && pos.x() < far.x()
            && pos.y() > self.base.y()
            && pos.y() < far.y()
            && pos.z() > self.base.z()
            && pos.z() < far.z()
    }

    /// Checks if the AABB contains the point, as if it's a closed interval
    /// (uses exclusive comparisons).
    pub fn contains_closed(&self, pos: Vec3f) -> bool {
        let far = self.get_far_corner();
        pos.x() >= self.base.x()
            && pos.x() <= far.x()
            && pos.y() >= self.base.y()
            && pos.y() <= far.y()
            && pos.z() >= self.base.z()
            && pos.z() <= far.z()
    }
}

/// Represents a collision between a subject AABB and a target AABB.
///
/// This correctly preserves the apparent travel direction, by storing a
/// travel dir and asserting that the depth is positive on construction.
#[derive(Copy, Clone, Debug)]
pub struct RawCollision {
    /// The depth that the subject penetrated into the target.
    depth: f32,

    travel_dir: Direction,
}

impl RawCollision {
    /// Depth the subject penetrated into the target.
    ///
    /// This will always be positive.
    pub fn penetration_depth(&self) -> f32 {
        self.depth
    }

    /// Apparent direction the subject was travelling.
    pub fn travelling_dir(&self) -> Direction {
        self.travel_dir
    }

    /// Penetration depth of the subject AABB.
    ///
    /// This is oriented in the direction that we were apparently travelling
    /// into the target AABB.  This isn't necessarily reflective of the true
    /// velocity, since it actually corresponds with the side that we
    /// intersected with it the shallowest.
    ///
    /// This gets weird with grazing collisions and might appear to point the
    /// wrong way.
    pub fn penetration_vec(&self) -> AAVec3f {
        self.travelling_dir().to_aa_vec(self.penetration_depth())
    }

    /// Returns a vec that would move the subject to be just adjacent to the
    /// target if offset by it.  This is just the negation of the penetration
    /// vec.
    pub fn undo_vec(&self) -> AAVec3f {
        -self.penetration_vec()
    }
}

/// Checks for a collision of a subject AABB into a target AABB.
pub fn check_aabb_collision(subject: &AABB, target: &AABB) -> Option<RawCollision> {
    let (adj_center, mk_diff) = compute_minkowski_diff(subject, target);
    #[cfg(test)]
    eprintln!("{adj_center} {mk_diff:?}");

    // Check each of the collisions so that we can figure it out.
    let xcol = check_axis(&mk_diff, adj_center, Axis::X)?;
    let ycol = check_axis(&mk_diff, adj_center, Axis::Y)?;
    let zcol = check_axis(&mk_diff, adj_center, Axis::Z)?;

    // Copy them into an array so that we can take `min_by` on them.
    let cols = [xcol, ycol, zcol];
    #[cfg(test)]
    eprintln!("{cols:#?}");
    let best = cols
        .into_iter()
        .min_by(|lhs, rhs| f32::total_cmp(&lhs.penetration_depth(), &rhs.penetration_depth()))
        .unwrap();

    Some(best)
}

/// Checks for a collision of a point into a target AABB.
pub fn check_point_collision(point: Vec3f, target: &AABB) -> Option<RawCollision> {
    let xcol = check_axis(target, point, Axis::X)?;
    let ycol = check_axis(target, point, Axis::Y)?;
    let zcol = check_axis(target, point, Axis::Z)?;

    // Copy them into an array so that we can take `min_by` on them.
    let cols = [xcol, ycol, zcol];
    #[cfg(test)]
    eprintln!("{cols:#?}");
    let best = cols
        .into_iter()
        .min_by(|lhs, rhs| f32::total_cmp(&lhs.penetration_depth(), &rhs.penetration_depth()))
        .unwrap();

    Some(best)
}

/// Given a subject that we're checking collisions on and a target AABB to
/// collide against, returns the adjusted center of the subject AABB and the
/// minkowski differenced AABB of the target.
///
/// This isn't a true minkowski difference, as that would have the difference
/// box aligned at the origin and the collider point off somewhere relative to
/// that, and that math is more complicated than I feel like doing right now.
fn compute_minkowski_diff(subject: &AABB, target: &AABB) -> (Vec3f, AABB) {
    let updated_target = target.expand_extents_by(subject.dims());
    (subject.get_center(), updated_target)
}

/// Checks on an axis if a collision could have happened, returning the
/// collision data if it would have happened on that axis.
#[inline(always)]
fn check_axis(mk_diff: &AABB, adj_center: Vec3f, axis: Axis) -> Option<RawCollision> {
    // Do some math to get each component.
    let min = mk_diff.base().component(axis);
    let dim = mk_diff.dims().component(axis);
    let max = min + dim;
    let adj_pos = adj_center.component(axis);

    #[cfg(test)]
    eprintln!("{axis:?}: {adj_pos} in {min}..{max}");

    // Abort immediately if we're not inside the box here, we're definitely
    // outside the box.
    if adj_pos < min || adj_pos > max {
        return None;
    }

    // Depending on which side of the AABB we're at, we get different numbers.
    let midpoint = min + dim / 2.0;
    let (travel_dir, depth) = if adj_pos < midpoint {
        // On negative side, we travel positive.
        (axis.positive_dir(), adj_pos - min)
    } else {
        // On positive side, we travel negative.
        (axis.negative_dir(), max - adj_pos)
    };

    // If depth is somehow NaN or infinite, assume no collision.
    if depth.is_nan() || depth.is_infinite() {
        return None;
    }

    // Another sanity check to make sure we're doing everything right.
    // Maybe this could be converted into a clamp to 0?
    assert!(
        depth >= 0.0,
        "aabb: penetration depth must be positive (depth: {depth})"
    );

    Some(RawCollision { travel_dir, depth })
}

#[cfg(test)]
mod tests {
    use crate::geom::{Direction, Vec3f};

    use super::{check_aabb_collision, compute_minkowski_diff, AABB};

    /// Checks if two vectors are similar up to some threshold.
    fn is_vec_approx_eq(a: Vec3f, b: Vec3f, thresh: f32) -> bool {
        (b - a).magnitude() < thresh
    }

    fn is_f32_approx_eq(a: f32, b: f32, thresh: f32) -> bool {
        (b - a).abs() < thresh
    }

    #[test]
    fn test_mkdiff() {
        let subj_center = Vec3f::new(2.0, 2.0, 2.0);
        let subj_dims = Vec3f::new(1.0, 1.0, 1.0);
        let subj = AABB::from_center(subj_center, subj_dims);
        let target_center = subj_center + Vec3f::new_y(3.0);
        let target_dims = Vec3f::new(1.0, 2.0, 3.0);
        let target = AABB::from_center(target_center, target_dims);

        let (off, mkdiff) = compute_minkowski_diff(&subj, &target);
        eprintln!("{off} {mkdiff:?}");

        assert!(is_vec_approx_eq(off, subj_center, 0.001));
        assert!(is_vec_approx_eq(
            mkdiff.dims(),
            target_dims + subj_dims,
            0.001
        ));

        // This is the output from the test, it should be right right?
        assert!(is_vec_approx_eq(
            Vec3f::new(1.0, 3.5, 0.0),
            mkdiff.base(),
            0.001
        ));
    }

    #[test]
    fn test_collision_simple() {
        let bb1 = AABB::from_center((1.05, 0.0, 0.0).into(), (0.2, 0.2, 0.2).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.05, 0.001));
        assert_eq!(col.travelling_dir(), Direction::West);
    }

    #[test]
    fn test_collision_grazing_px() {
        let bb1 = AABB::from_center((1.5, 0.0, 0.0).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::West);
    }

    #[test]
    fn test_collision_grazing_nx() {
        let bb1 = AABB::from_center((-1.5, 0.0, 0.0).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::East);
    }

    #[test]
    fn test_collision_grazing_py() {
        let bb1 = AABB::from_center((0.0, 1.5, 0.0).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::Down);
    }

    #[test]
    fn test_collision_grazing_ny() {
        let bb1 = AABB::from_center((0.0, -1.5, 0.0).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::Up);
    }

    #[test]
    fn test_collision_grazing_pz() {
        let bb1 = AABB::from_center((0.0, 0.0, 1.5).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::North);
    }

    #[test]
    fn test_collision_grazing_nz() {
        let bb1 = AABB::from_center((0.0, 0.0, -1.5).into(), (1.0, 1.0, 1.0).into());
        let bb2 = AABB::new_origin((2.0, 2.0, 2.0).into());
        let col = check_aabb_collision(&bb1, &bb2).expect("aabb: find collision");
        eprintln!("{col:?}");
        assert!(is_f32_approx_eq(col.penetration_depth(), 0.0, 0.001));
        assert_eq!(col.travelling_dir(), Direction::South);
    }

    // TODO askew collision with slightly more complicated checks
}
